﻿using System.Collections;
using System.Collections.Generic;
using UnityEngine;

namespace NinjaToolbox.PhysicsSystem
{
    public static class PhysicsUtil
    {
        public struct StepMoveOptions
        {
            public float StepLength;
            public float Error;
            public int MaxIterateCount;
            public int LayerMask;


            public static StepMoveOptions Create(float stepLength = 0.3f)
            {
                return new StepMoveOptions()
                {
                    StepLength = stepLength,
                    Error = 0.05f,
                    MaxIterateCount = 600,
                    LayerMask = ~0,
                };
            }
        }

        public static readonly float INFINITY = 100000.0f;
        public static readonly Color PHYSICS_SHAPE_GIZMOS_COLOR = new Color(145f, 244f, 139f, 210f) / 255f;


        public static bool StepMoveTo(PhysicsObject physicsObject, Vector3 to, StepMoveOptions options)
        {
            if (physicsObject.PhysicsShape.IsContainPoint(to)) return false;

            var edgePoint = Vector3.zero;

            edgePoint = physicsObject.PhysicsShape.MassPointToEdge(physicsObject.transform.position, to);

            var moveValue = to - edgePoint;
            var firstIntersect = default(IPhysicsShape);

            return StepMove(physicsObject, moveValue, out firstIntersect, options);
        }

        public static bool StepMove(PhysicsObject physicsObject, Vector3 moveValue, StepMoveOptions options)
        {
            var firstIntersect = default(IPhysicsShape);

            return StepMove(physicsObject, moveValue, out firstIntersect, options);
        }

        public static bool StepMove(PhysicsObject physicsObject, Vector3 moveValue, out IPhysicsShape firstIntersect, StepMoveOptions options)
        {
            var result = true;
            firstIntersect = default(IPhysicsShape);

            var direction = moveValue.normalized;
            var stepLength = options.StepLength;
            var step = direction * stepLength;

            if (step.magnitude > moveValue.magnitude)
                step = moveValue;

            var to = physicsObject.transform.position + moveValue;
            var errorCounter = 0;

            var canNext = true;
            while (canNext)
            {
                errorCounter++;
                if (errorCounter > options.MaxIterateCount)
                {
                    Debug.LogError("StepMove While Crashed!");
                    result = false;
                    break;
                }
                //如果超出最大迭代数量就报错

                var isIntersectWithSceneColliders = false;
                var cachePosition = physicsObject.transform.position;
                physicsObject.transform.position += step;
                isIntersectWithSceneColliders = PhysicsSystemManager.Instance.IsIntersectWithSceneColliders(physicsObject.PhysicsShape, out firstIntersect, options.LayerMask);
                physicsObject.transform.position = cachePosition;

                if (isIntersectWithSceneColliders)
                {
                    canNext = false;
                    result = false;
                }
                else
                {
                    physicsObject.transform.position += step;//执行一次移动
                }

                if (canNext)//距离跳出判断
                    canNext = Vector3.Distance(physicsObject.transform.position, to) > options.Error;

                if (canNext)//方向跳出判断
                {
                    var currentDirection = (to - physicsObject.transform.position).normalized;
                    var dot = Vector3.Dot(direction, currentDirection);
                    if (dot < 0.1f)
                        canNext = false;
                }
            }

            return result;
        }

        public static bool BoxVSBox(Vector3 xP0, Vector3 xP1, Vector3 xP2, Vector3 xP3, Vector3 xP4, Vector3 xP5, Vector3 xP6, Vector3 xP7
                                                    , Vector3 yP0, Vector3 yP1, Vector3 yP2, Vector3 yP3, Vector3 yP4, Vector3 yP5, Vector3 yP6, Vector3 yP7
                                                    , Vector3 x_XAxis, Vector3 x_YAxis, Vector3 x_ZAxis
                                                    , Vector3 y_XAxis, Vector3 y_YAxis, Vector3 y_ZAxis)
        {
            var isNotIntersect = false;
            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, x_XAxis);
            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, x_YAxis);
            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, x_ZAxis);
            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, y_XAxis);
            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, y_YAxis);
            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, y_ZAxis);

            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, Vector3.Cross(x_XAxis, y_XAxis));
            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, Vector3.Cross(x_XAxis, y_YAxis));
            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, Vector3.Cross(x_XAxis, y_ZAxis));

            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, Vector3.Cross(x_YAxis, y_XAxis));
            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, Vector3.Cross(x_YAxis, y_YAxis));
            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, Vector3.Cross(x_YAxis, y_ZAxis));

            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, Vector3.Cross(x_ZAxis, y_XAxis));
            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, Vector3.Cross(x_ZAxis, y_YAxis));
            isNotIntersect |= ProjectionIsNotIntersect(xP0, xP1, xP2, xP3, xP4, xP5, xP6, xP7, yP0, yP1, yP2, yP3, yP4, yP5, yP6, yP7, Vector3.Cross(x_ZAxis, y_ZAxis));

            return isNotIntersect ? false : true;
        }

        public static bool BoxVSSphere(Matrix4x4 xMatrix, Vector3 xOffset, Vector3 xSize, Vector3 yCenter, float yRadius)
        {
            var localSpherePoint = xMatrix.inverse.MultiplyPoint3x4(yCenter);

            var sizeHalf = xSize * 0.5f;

            var xMin = xOffset.x + (-sizeHalf.x);
            var xMax = xOffset.x + sizeHalf.x;
            var yMin = xOffset.y + (-sizeHalf.y);
            var yMax = xOffset.y + sizeHalf.y;
            var zMin = xOffset.z + (-sizeHalf.z);
            var zMax = xOffset.z + sizeHalf.z;

            var outside = false;
            if (localSpherePoint.x < xMin)
            {
                outside = true;
                localSpherePoint.x = xMin;
            }
            else if (localSpherePoint.x > xMax)
            {
                outside = true;
                localSpherePoint.x = xMax;
            }

            if (localSpherePoint.y < yMin)
            {
                outside = true;
                localSpherePoint.y = yMin;
            }
            else if (localSpherePoint.y > yMax)
            {
                outside = true;
                localSpherePoint.y = yMax;
            }

            if (localSpherePoint.z < zMin)
            {
                outside = true;
                localSpherePoint.z = zMin;
            }
            else if (localSpherePoint.z > zMax)
            {
                outside = true;
                localSpherePoint.z = zMax;
            }

            if (outside)
            {
                var edgePoint = xMatrix.MultiplyPoint3x4(localSpherePoint);
                var distance = Vector3.Distance(yCenter, edgePoint);

                if (distance > yRadius)
                    return false;
            }

            return true;
        }

        public static bool SphereVSSphere(Vector3 xCenter, float xRadius, Vector3 yCenter, float yRadius)
        {
            return Vector3.Distance(xCenter, yCenter) <= (xRadius + yRadius);
        }

        public static bool BoxVSVirtualCapsule(Matrix4x4 xMatrix, Vector3 xOffset, Vector3 xSize
                                                                    , Matrix4x4 yMatrix, Vector3 yOffset, float yHeight, float yRadius
                                                                    , VirtualCapsuleShape.DirectionEnum yDirection, int yVirtualSegment)
        {
            var up = Vector3.up;

            switch (yDirection)
            {
                case VirtualCapsuleShape.DirectionEnum.X:
                    up = Vector3.right;
                    break;
                case VirtualCapsuleShape.DirectionEnum.Z:
                    up = Vector3.forward;
                    break;
            }

            var lossyScale = yMatrix.lossyScale;
            var maxScale = Mathf.Max(Mathf.Max(Mathf.Abs(lossyScale.x), Mathf.Abs(lossyScale.y)), Mathf.Abs(lossyScale.z));
            yRadius *= maxScale;
            yHeight *= maxScale;

            var remainHeight = Mathf.Max(yHeight - yRadius * 2f, 0) + 0.01f;
            var remainHeightHalf = remainHeight * 0.5f;
            up = yMatrix.MultiplyVector(up);
            yOffset = yMatrix.MultiplyPoint3x4(yOffset);

            if (BoxVSSphere(xMatrix, xOffset, xSize, yOffset + up * remainHeightHalf, yRadius))
                return true;

            if (BoxVSSphere(xMatrix, xOffset, xSize, yOffset - up * remainHeightHalf, yRadius))
                return true;

            var bottom = yOffset - yHeight * 0.5f * up;
            var top = yOffset + yHeight * 0.5f * up;
            var step = (top - bottom) / Mathf.Max(1, yVirtualSegment + 1);

            for (int i = 1; i <= yVirtualSegment; i++)
            {
                var spherePosition = bottom + step * i;

                if (BoxVSSphere(xMatrix, xOffset, xSize, spherePosition, yRadius))
                    return true;
            }

            return false;
        }

        public static bool SphereVSVirtualCapsule(Vector3 xCenter, float xRadius
                                                                    , Matrix4x4 yMatrix, Vector3 yOffset, float yHeight, float yRadius
                                                                    , VirtualCapsuleShape.DirectionEnum yDirection, int yVirtualSegment)
        {
            var up = Vector3.up;

            switch (yDirection)
            {
                case VirtualCapsuleShape.DirectionEnum.X:
                    up = Vector3.right;
                    break;
                case VirtualCapsuleShape.DirectionEnum.Z:
                    up = Vector3.forward;
                    break;
            }

            var lossyScale = yMatrix.lossyScale;
            var maxScale = Mathf.Max(Mathf.Max(Mathf.Abs(lossyScale.x), Mathf.Abs(lossyScale.y)), Mathf.Abs(lossyScale.z));
            yRadius *= maxScale;
            yHeight *= maxScale;

            var remainHeight = Mathf.Max(yHeight - yRadius * 2f, 0) + 0.01f;
            var remainHeightHalf = remainHeight * 0.5f;
            up = yMatrix.MultiplyVector(up);
            yOffset = yMatrix.MultiplyPoint3x4(yOffset);

            if (SphereVSSphere(xCenter, xRadius, yOffset + up * remainHeightHalf, yRadius))
                return true;

            if (SphereVSSphere(xCenter, xRadius, yOffset - up * remainHeightHalf, yRadius))
                return true;

            var bottom = yOffset - yHeight * 0.5f * up;
            var top = yOffset + yHeight * 0.5f * up;
            var step = (top - bottom) / Mathf.Max(1, yVirtualSegment + 1);

            for (int i = 1; i <= yVirtualSegment; i++)
            {
                var spherePosition = bottom + step * i;

                if (SphereVSSphere(xCenter, xRadius, spherePosition, yRadius))
                    return true;
            }

            return false;
        }

        public static bool VirtualCapsuleVSVirtualCapsule(
                                                                    Matrix4x4 xMatrix, Vector3 xOffset, float xHeight, float xRadius
                                                                    , VirtualCapsuleShape.DirectionEnum xDirection, int xVirtualSegment

                                                                    , Matrix4x4 yMatrix, Vector3 yOffset, float yHeight, float yRadius
                                                                    , VirtualCapsuleShape.DirectionEnum yDirection, int yVirtualSegment)
        {
            var up = Vector3.up;

            switch (xDirection)
            {
                case VirtualCapsuleShape.DirectionEnum.X:
                    up = Vector3.right;
                    break;
                case VirtualCapsuleShape.DirectionEnum.Z:
                    up = Vector3.forward;
                    break;
            }

            up = xMatrix.MultiplyVector(up);

            var lossyScale = xMatrix.lossyScale;
            var maxScale = Mathf.Max(Mathf.Max(Mathf.Abs(lossyScale.x), Mathf.Abs(lossyScale.y)), Mathf.Abs(lossyScale.z));
            xRadius *= maxScale;
            xHeight *= maxScale;

            var remainHeight = Mathf.Max(xHeight - xRadius * 2f, 0) + 0.01f;
            var remainHeightHalf = remainHeight * 0.5f;
            xOffset = xMatrix.MultiplyPoint3x4(xOffset);

            if (SphereVSVirtualCapsule(xOffset + up * remainHeightHalf, xRadius, yMatrix, yOffset, yHeight, yRadius, yDirection, yVirtualSegment))
                return true;

            if (SphereVSVirtualCapsule(xOffset - up * remainHeightHalf, xRadius, yMatrix, yOffset, yHeight, yRadius, yDirection, yVirtualSegment))
                return true;

            var bottom = xOffset - xHeight * 0.5f * up;
            var top = xOffset + xHeight * 0.5f * up;
            var step = (top - bottom) / Mathf.Max(1, xVirtualSegment + 1);

            for (int i = 1; i <= xVirtualSegment; i++)
            {
                var spherePosition = bottom + step * i;

                if (SphereVSVirtualCapsule(spherePosition, xRadius, yMatrix, yOffset, yHeight, yRadius, yDirection, yVirtualSegment))
                    return true;
            }

            return false;
        }

        static bool ProjectionIsNotIntersect(Vector3 xP0, Vector3 xP1, Vector3 xP2, Vector3 xP3, Vector3 xP4, Vector3 xP5, Vector3 xP6, Vector3 xP7
                                                                  , Vector3 yP0, Vector3 yP1, Vector3 yP2, Vector3 yP3, Vector3 yP4, Vector3 yP5, Vector3 yP6, Vector3 yP7
                                                                  , Vector3 axis)
        {
            var x_p0 = GetSignProjectValue(xP0, axis);
            var x_p1 = GetSignProjectValue(xP1, axis);
            var x_p2 = GetSignProjectValue(xP2, axis);
            var x_p3 = GetSignProjectValue(xP3, axis);
            var x_p4 = GetSignProjectValue(xP4, axis);
            var x_p5 = GetSignProjectValue(xP5, axis);
            var x_p6 = GetSignProjectValue(xP6, axis);
            var x_p7 = GetSignProjectValue(xP7, axis);

            var y_p0 = GetSignProjectValue(yP0, axis);
            var y_p1 = GetSignProjectValue(yP1, axis);
            var y_p2 = GetSignProjectValue(yP2, axis);
            var y_p3 = GetSignProjectValue(yP3, axis);
            var y_p4 = GetSignProjectValue(yP4, axis);
            var y_p5 = GetSignProjectValue(yP5, axis);
            var y_p6 = GetSignProjectValue(yP6, axis);
            var y_p7 = GetSignProjectValue(yP7, axis);

            var xMin = Mathf.Min(x_p0, Mathf.Min(x_p1, Mathf.Min(x_p2, Mathf.Min(x_p3, Mathf.Min(x_p4, Mathf.Min(x_p5, Mathf.Min(x_p6, x_p7)))))));
            var xMax = Mathf.Max(x_p0, Mathf.Max(x_p1, Mathf.Max(x_p2, Mathf.Max(x_p3, Mathf.Max(x_p4, Mathf.Max(x_p5, Mathf.Max(x_p6, x_p7)))))));
            var yMin = Mathf.Min(y_p0, Mathf.Min(y_p1, Mathf.Min(y_p2, Mathf.Min(y_p3, Mathf.Min(y_p4, Mathf.Min(y_p5, Mathf.Min(y_p6, y_p7)))))));
            var yMax = Mathf.Max(y_p0, Mathf.Max(y_p1, Mathf.Max(y_p2, Mathf.Max(y_p3, Mathf.Max(y_p4, Mathf.Max(y_p5, Mathf.Max(y_p6, y_p7)))))));

            if (yMin >= xMin && yMin <= xMax) return false;
            if (yMax >= xMin && yMax <= xMax) return false;
            if (xMin >= yMin && xMin <= yMax) return false;
            if (xMax >= yMin && xMax <= yMax) return false;

            return true;
        }

        static float GetSignProjectValue(Vector3 point, Vector3 axis)
        {
            var projectPoint = Vector3.Project(point, axis);
            var result = projectPoint.magnitude * Mathf.Sign(Vector3.Dot(projectPoint, axis));

            return result;
        }
    }
}
